
#include <gsl_ext_cpp/GSLFunctionWrapper.h>
#include <gsl_ext_cpp/GSLMinimizerWrapper.h>
#include <gsl_ext_cpp/GSLVectorWrapper.h>
#include <gsl_ext_oct/MinimizerStateCollector.h>
#include <gsl_ext_oct/OctaveArgParser.h>
#include <gsl_ext_oct/OctaveUtils.h>

#include <gsl/gsl_multimin.h>
#include <iomanip>
#include <octave/oct.h>
#include <octave/oct-map.h>
#include <stdexcept>
#include <sstream>
#include <string>
#include <time.h>
#include <vector>

using namespace std;

static const unsigned int MAX_NUM_ITER = 50000;
static const unsigned long long MIN_TOTAL_TIME = 1e9;

static const string ERROR_PREFIX = "GSLpp_minimize: ";

static bool checkArgs(const octave_value_list &args)
{
  const int nargin = args.length();
  
  if(nargin < 11)
  {
    error((ERROR_PREFIX + "11 parameters required.").c_str());
    return false;
  }
  
  if(!args(0).is_string())
  {
    usage((ERROR_PREFIX + "The first parameter (algorithm name) must be a string.").c_str());
    return false;
  }
  
  if(!args(1).is_map())
  {
    usage((ERROR_PREFIX + "The second parameter (algorithm parameters) must be a struct.").c_str());
    return false;
  }
  
  if(!args(2).is_map())
  {
    usage((ERROR_PREFIX + "The third parameter (line search parameters) must be a struct.").c_str());
    return false;
  }
  
  if(!args(3).is_string())
  {
    usage((ERROR_PREFIX + "The fourth parameter (objective function) must be a string.").c_str());
    return false;
  }
  
  if(!args(4).is_map())
  {
    usage((ERROR_PREFIX + "The fifth parameter (objective function parameters) must be a struct.").c_str());
    return false;
  }
  
  if(!args(5).is_map())
  {
    usage((ERROR_PREFIX + "The sixth parameter (evaluation method) must be a struct.").c_str());
    return false;
  }
  
  if(!args(6).is_string())
  {
    usage((ERROR_PREFIX + "The seventh parameter (stopping criterion name) must be a string.").c_str());
    return false;
  }
  
  if(!args(7).is_map())
  {
    usage((ERROR_PREFIX + "The eighth parameter (stopping criterion parameters) must be a struct.").c_str());
    return false;
  }
  
  if(!args(8).is_matrix_type())
  {
    usage((ERROR_PREFIX + "The ninth parameter (starting point) must be a vector.").c_str());
    return false;
  }
  
  if(!args(9).is_real_type())
  {
    usage((ERROR_PREFIX + "The tenth parameter (verbosity level) must be an integer.").c_str());
    return false;
  }
  int verbosity = args(9).double_value();
  
  if(!args(10).is_bool_type())
  {
    usage((ERROR_PREFIX + "The eleventh parameter (time test mode) must be a boolean.").c_str());
    return false;
  }
  
  return true;
}

gsl_vector *getInitSimplex(int n, const Octave_map &minimizerParams)
{
  gsl_vector *initSimplex = gsl_vector_alloc(n);
  if(!minimizerParams.contains("initsimplexsize"))
  {
    for(int i = 0; i < n; i++)
      gsl_vector_set(initSimplex, i, 0.9);
  }
  else
  {
    ColumnVector iss = ColumnVector(minimizerParams.seek("initsimplexsize")->second(0).vector_value());
    if(iss.length() != n)
    {
      error((ERROR_PREFIX + "Invalid initial simplex dimensions.").c_str());
      return NULL;
    }
    for(int i = 0; i < n; i++)
      gsl_vector_set(initSimplex, i, iss(i));
  }
  return initSimplex;
}

static unsigned long long getTime()
{
  timespec tp;
  clock_gettime(CLOCK_REALTIME, &tp);
  return 1e9 * tp.tv_sec + tp.tv_nsec;
}

static void printSummary(int k, const GSLMinimizerWrapper &s, const GSLFunctionWrapper &f)
{
  cout<<endl;
  cout<<left<<setw(35)<<"Minimizer:"<<GSLVectorWrapper(s.x())<<endl;
  cout<<setw(35)<<"Minimum value:"<<s.f()<<endl;
  if(s.getType() == GSLMinimizerWrapper::FDF || 
     s.getType() == GSLMinimizerWrapper::FD2F)
    cout<<setw(35)<<"Minimum gradient:"<<GSLVectorWrapper(s.gradient())<<endl;
  cout<<setw(35)<<"iterations:"<<k<<endl;
  cout<<setw(35)<<"function evaluations:"<<f.getFuncEvalCounter()<<endl;
  if(s.getType() == GSLMinimizerWrapper::FDF || 
     s.getType() == GSLMinimizerWrapper::FD2F)
    cout<<setw(35)<<"gradient evaluations:"<<f.getGradEvalCounter()<<" ("<<f.n() * f.getGradEvalCounter()<<")"<<endl;
  if(s.getType() == GSLMinimizerWrapper::FD2F)
    cout<<setw(35)<<"Hessian evaluations:"<<f.getHessEvalCounter()<<" ("<<f.n() * (f.n() + 1) / 2.0 * f.getHessEvalCounter()<<")"<<endl;
  cout<<endl;
}

static void printResultsTableHeader(const GSLMinimizerWrapper &s, const GSLFunctionWrapper &f)
{
  int i;
  vector< string > varNames;
  
  cout<<setw(6)<<left<<"k";
  
  f.getVariableNames(varNames);
  for(i = 0; i < f.n(); i++)
    cout<<setw(20)<<left<<varNames[i];
  
  cout<<"f(";
  for(i = 0; i < f.n() - 1; i++)
    cout<<varNames[i]<<",";
  cout<<varNames[f.n() - 1];
  cout<<")"<<endl;
}

static void printResultsTableRow(int k, const GSLMinimizerWrapper &s, const GSLFunctionWrapper &f)
{
  cout<<setw(6)<<left<<k;
  for(int i = 0; i < f.n(); i++)
    cout<<setw(20)<<left<<gsl_vector_get(s.x(), i);
  
  cout<<setw(8)<<left<<s.f()<<endl;
}

DEFUN_DLD(GSLpp_minimize, args, nargout, " -*- texinfo -*-\n\
@deftypefn {Loadable Function} {[@var{results}] =} GSLpp_minimize (@var{a}, @var{ap}, @var{lsp}, @var{f}, @var{fp}, @var{ep}, @var{sc}, @var{scp}, @var{sp}, @var{v}, @var{t})\n\
Invokes a GSL++ or GSL minimization algorithm.\n\
\n\
Arguments:\n\n\
* A: string that specifies the name of the minimizer to use\n\n\
The minimization algorithms implemented in GSL++ are:\n\n\
@multitable @columnfractions .3 .3 .2 .2 \n\
@headitem Name @tab Method @tab Line search @tab Initial step \n\
@item bfgs_f @tab quasi-Newton BFGS / inverse Hessian update @tab Fletcher @tab unity \n\
@item bfgs_hess_f @tab quasi-Newton BFGS / Hessian update @tab Fletcher @tab unity \n\
@item bfgs_mt @tab quasi-Newton BFGS / inverse Hessian update @tab More and Thuente @tab unity \n\
@item bfgs_hess_mt @tab quasi-Newton BFGS / Hessian update @tab More and Thuente @tab unity \n\
@item conjgrad_fr_mt @tab Fletcher-Reeves conjugate gradient @tab More and Thuente @tab Fletcher \n\
@item conjgrad_pr_mt @tab Polak-Ribiere conjugate gradient @tab More and Thuente @tab Fletcher \n\
@item lrwwsimplex @tab Nelder and Mead simplex (revised) @tab N/A @tab N/A \n\
@item mnewton @tab Newton / modified Cholesky @tab More and Thuente @tab unity \n\
@end multitable \n\n\
* AP: a struct with minimizer parameters\n\n\
@multitable @columnfractions .33 .33 .33 \n\
@headitem Parameter @tab Description @ Values \n\
@item linmin @tab line search algorithm @tab fletcher,morethuente\n\
@end multitable \n\n\
* LSP: a struct with line minimizer parameters\n\n\
@multitable @columnfractions .3 .7 \n\
@headitem Algorithm @tab Parameters \n\
@item Fletcher @tab mu,eta,tau,xi \n\
@item More and Thuente @tab mu,eta,xi \n\
@end multitable \n\n\
* F: string that specifies the function to minimize\n\n\
GSL++ accepts either symbolic expressions or names of predefined\n\
functions in the More, Garbow and Hillstrom test function set.\n\
Symbolic expressions are parsed with GNU libmatheval, see its\n\
manual for details. The predefined test functions are:\n\n\
@multitable @columnfractions .6 .4 \n\
@headitem Name @tab Dimensions \n\
@item powellbadlyscaled @tab n=2, m=2 \n\
@item brownbadlyscaled @tab n=2, m=3 \n\
@item beale @tab n=2, m=3 \n\
@item helicalvalley @tab n=3, m=3 \n\
@item gaussian @tab n=3, m=15 \n\
@item gulf @tab n=3, n<=m<=100 \n\
@item box @tab n=3, m>=n \n\
@item wood @tab n=4, m=6\n\
@item browndennis @tab n=4, m>=n \n\
@item biggsexp6 @tab n=6, m>=n \n\
@item watson @tab 2<=n<=31, m=31 \n\
@item extendedrosenbrock @tab n even, m=n \n\
@item extendedpowellsingular @tab n multiple of 4, m=n \n\
@item penaltyI @tab n arbitrary, m=n+1 \n\
@item penaltyII @tab n arbitrary, m=2n \n\
@item variablydimensioned @tab n arbitrary, m=n+2 \n\
@item trigonometric @tab n arbitrary, m=n \n\
@item chebyquad @tab n arbitrary, m>=n \n\
@end multitable \n\n\
* FP: a struct that specifies objective function parameters, \n\
e.g. dimensions. These are specified with m and n.\n\n\
* EP: a struct that specifies function evaluation method\n\n\
The function evaluation method is specified with a struct containing \n\
the fields f, g, and H. The following combinations \n\
of evaluation methods are allowed:\n\n\
@multitable @columnfractions .33 .33 .33 \n\
@headitem Function @tab Gradient @tab Hessian \n\
@item sym @tab sym @tab sym \n\
@item sym @tab sym @tab fd \n\
@item sym @tab fd @tab fd \n\
@item precomp @tab fd @tab fd \n\
@end multitable \n\n\
where 'sym' refers to symbolic evaluation, 'fd' refers to\n\
finite-difference approximation and precomp refers to a\n\
precompiled test function.\n\n\
* SC: the name of the stopping criterion to use\n\n\
@multitable @columnfractions .2 .8 \n\
@item disttomin @tab distance to the minimizer (if it is a priori known)\n\
@item disttominval @tab distance to the minimum function value (if it is a priori known)\n\
@item fdiff @tab difference of two successive function values (f(x_k)-f(x_k+1)) \n\
@item gradient @tab gradient norm \n\
@item maxnumiter @tab maximum number of iterations \n\
@item simplexsize @tab simplex size (only applicable to gsl_nmsimplex) \n\
@item simplexvolume @tab simplex volume (only applicable to gsl_ext_lrwwsimplex) \n\
@item xdiff @tab difference of two successive iterates (||x_k+1-x_k||) \n\
@end multitable \n\n\
* SCP: a struct stopping of criterion parameters, e.g. threshold value\n\
specified with 'threshold'\n\n\
* SP: starting point vector\n\n\
* V: verbosity level\n\
@multitable @columnfractions .2 .8 \n\
@item 0 @tab no output \n\
@item 1 @tab basic information, i.e. number of iterations and function evaluations \n\
@item 2 @tab basic information + summary of iterations \n\
@item 3 @tab full iteration information, i.e. details of each iteration \n\
@end multitable \n\n\
* T: time test mode\n\n\
If time test mode is enabled, the used computation time is measured as\n\
the average of successive runs\n\
@end deftypefn")
{
  bool converged = false;
  Octave_map curIterMap;
  unsigned int i;
  Cell iterPath(MAX_NUM_ITER, 1);
  unsigned int k = 0;
  Octave_map results;
  octave_value_list retval;
  unsigned int numRuns = 0;
  int status;
  unsigned long long startTime;
  unsigned long long totalTime = 0;
  double usedTime;
  
  const int nargin = args.length();
  
  if(!checkArgs(args))
    return retval;
  
  string minimizerName = args(0).string_value();
  Octave_map minimizerParams = args(1).map_value();
  Octave_map lineMinimizerParams = args(2).map_value();
  string testFuncName = args(3).string_value();
  Octave_map testFuncParams = args(4).map_value();
  Octave_map evalParams = args(5).map_value();
  Octave_map evalParams_fd2f;
  string stopCritName = args(6).string_value();
  Octave_map stopCritParams = args(7).map_value();	
  ColumnVector startingPoint = ColumnVector(args(8).vector_value());
  int verbosity = args(9).double_value();	
  bool timeTest = args(10).bool_value();
	
  double stepSize, tol;
  
  GSLFunctionWrapper *f;
  GSLFunctionWrapper_fd2f *f_fd2f;
  gsl_error_handler_t *oldGSLErrorHandler;
  GSLMinimizerWrapper *s;
  StoppingCriterion *stopCrit;
  gsl_vector *initSimplex = NULL;
  gsl_vector *x0;
  MinimizerStateCollector *stateCollector;
  
  oldGSLErrorHandler = gsl_set_error_handler_off();
  
  gsl_ext_testfunction_params func_params;
  try {
    f = OctaveArgParser::getFunctionInstance(testFuncName,
                                             testFuncParams,
                                             evalParams,
                                             OctaveArgParser::getGSLFunctionType(minimizerName),
                                             &func_params);
    if(timeTest == false)
    {
      evalParams_fd2f.assign("f", "sym");
      evalParams_fd2f.assign("g", "sym");
      evalParams_fd2f.assign("H", "sym");
      f_fd2f = dynamic_cast< GSLFunctionWrapper_fd2f * >(
        OctaveArgParser::getFunctionInstance(testFuncName,
                                             testFuncParams,
                                             evalParams_fd2f,
                                             GSLFunctionWrapper::FD2F,
                                             &func_params));
    }
  }
  catch(exception &e) {
    error((ERROR_PREFIX + "Unable to initialize the objective function. Reason: " + e.what()).c_str());
    return retval;
  }
  
  try {
    s = OctaveArgParser::getMinimizerInstance(minimizerName, minimizerParams, lineMinimizerParams, f->n(), stepSize, tol);
    stateCollector = MinimizerStateCollectorFactory::getInstance(minimizerName, f->n());
  }
  catch(exception &e) {
    error((ERROR_PREFIX + "Unable to initialize the minimization algorithm. Reason: " + e.what()).c_str());
    return retval;
  }
  
  try {
    stopCrit = OctaveArgParser::getStoppingCriterionInstance(stopCritName, stopCritParams);
  }
  catch(exception &e) {
    error((ERROR_PREFIX + "Unable to initialize stopping criterion. Reason: " + e.what()).c_str());
    return retval;
  }
  
  gsl_set_error_handler(oldGSLErrorHandler);
  
  if(!stopCrit->isCompatibleWith(*f))
  {
    error((ERROR_PREFIX + "Stopping criterion is not compatible with function. Check dimensions.").c_str());
    return retval;
  }
  
  if(!stopCrit->isCompatibleWith(*s))
  {
    error((ERROR_PREFIX + "Stopping criterion is not compatible with solver.").c_str());
    return retval;
  }
  
  x0 = OctaveUtils::OctaveVectorToGSLVector(startingPoint);
  
  if(f->n() != x0->size)
  {
    error((ERROR_PREFIX + "Dimension mismatch. Starting point is not compatible with function.").c_str());
    return retval;
  }
  
  if(verbosity >= 2)
    printResultsTableHeader(*s, *f);
  
  do
  {
    k = 0;
    converged = false;
    
    if(s->getType() == GSLMinimizerWrapper::F)
    {
      initSimplex = getInitSimplex(f->n(), minimizerParams);
      if(initSimplex == NULL)
      {
        error((ERROR_PREFIX + "No initial simplex given.").c_str());
        return retval;
      }
      dynamic_cast< GSLMinimizerWrapper_f * >(s)->set(*f, x0, initSimplex);
      gsl_vector_free(initSimplex);
    }
    else if(s->getType() == GSLMinimizerWrapper::FDF)
      dynamic_cast< GSLMinimizerWrapper_fdf * >(s)->set(*f, x0, stepSize, tol);
    else if(s->getType() == GSLMinimizerWrapper::FD2F)
      dynamic_cast< GSLMinimizerWrapper_fd2f * >(s)->set(*f, x0, stepSize, tol);
    
    if(timeTest == true)
      startTime = getTime();
    
    do
    {
      if(verbosity >= 2)
        printResultsTableRow(k, *s, *f);
      
      if(timeTest == false)
      {
        curIterMap.clear();
        stateCollector->collectStateData(*s, *f_fd2f, *stopCrit, curIterMap);
        iterPath(k, 0) = curIterMap;
      }

      if(status == GSL_CONTINUE && converged == true)
      {
        status = GSL_SUCCESS;
        break;
      }
      
      k++;
      status = s->iterate();
      
      if(status != GSL_SUCCESS)
      {
        converged = false;
        k--;
        break;
      }
      
      status = stopCrit->test(*s);
      
      if(status == GSL_SUCCESS)
      {
        status = GSL_CONTINUE;
        converged = true;
      }
    }
    while(status == GSL_CONTINUE && k < MAX_NUM_ITER);

    if(timeTest == true)
    {
      totalTime += getTime() - startTime;
      numRuns++;
    }
  }
  while(timeTest == true && totalTime < MIN_TOTAL_TIME);
  
  if(verbosity >= 1)
    printSummary(k, *s, *f);
  
  Octave_map inputMap;
  inputMap.assign("a",   args(0));
  inputMap.assign("ap",  args(1));
  inputMap.assign("lsp", args(2));
  inputMap.assign("f",   args(3));
  inputMap.assign("fp",  args(4));
  inputMap.assign("ep",  args(5));
  inputMap.assign("sc",  args(6));
  inputMap.assign("scp", args(7));
  inputMap.assign("sp",  args(8));
  inputMap.assign("v",   args(9));
  inputMap.assign("t",   args(10));
  
  iterPath.resize(dim_vector(k + 1, 1));
  results.assign("objfunc",   testFuncName);
  results.assign("input",     octave_value(inputMap));
  results.assign("converged", converged);
  results.assign("numiter",   min(k + 1, MAX_NUM_ITER));
  results.assign("nfeval",    f->getFuncEvalCounter());
  results.assign("ngeval",    f->getGradEvalCounter());
  results.assign("nHeval",    f->getHessEvalCounter());
  if(timeTest == false)
    results.assign("iterations", octave_value(iterPath));
  results.assign("termval", stopCrit->getThresholdValue(*s));
  
  if(timeTest == true)
  {
    usedTime = ((double)totalTime) / numRuns / 1000.0;
    if(converged)
      results.assign("time", usedTime);
    else
      results.assign("time", nan);
  }
  
  if(stateCollector != NULL)
    delete stateCollector;
  gsl_vector_free(x0);
  delete stopCrit;
  s->destroyWrappedObject();
  delete s;
  f->destroyWrappedObject();
  delete f;
  if(timeTest == false)
  {
    f_fd2f->destroyWrappedObject();
    delete f_fd2f;
  }
  
  return octave_value(results);
}
